# -*- coding: utf-8 -*-
"""

"""

import numpy as np

def geodetic2enu(center=(0, 0, 0), pos=(0, 0, 0)):
    """ WGS84坐标系转东北天坐标系
    
    东北天坐标系（ENU），属于常见的的站心地平直角坐标系，
    是一种本地的笛卡尔坐标系（通常称为局部切面坐标系）， 
    假设北方为 Y，东方为 X，单位为米。

    参数
    ----------
    center : tuple, optional
        选中用于建立指标坐标系的原点的经纬度, 形式为：
        :math:`(lon, lat, h)`，by default (0, 0, 0)
    pos : tuple, optional
        选择的需要转换的坐标点, 形式为 :math:`(lon, lat, h)`
        by default (0, 0, 0)

    返回值
    ----------
    tuple
        返回的转换后的坐标值，形式为 :math:`(x, y, z)`
    """
    
    # 解包元组中的值
    lat_center, lon_center, h_center = center
    lat, lon, h = pos
    
    # 地球半径（近似值）
    R = 6378137.0  # in meters
    
    # 将角度转换为弧度
    lat_center_rad = np.deg2rad(lat_center)
    lon_center_rad = np.deg2rad(lon_center)
    lat_rad = np.deg2rad(lat)
    lon_rad = np.deg2rad(lon)
    
    # 计算差值，这里使用弧度计算
    delta_lat = lat_rad - lat_center_rad
    delta_lon = lon_rad - lon_center_rad
    
    # 转换成米
    x = - delta_lon * np.cos(lat_center_rad) * R
    y = delta_lat * R
    z = h - h_center
    
    return x, y, z

import numpy as np

def enu2geodetic(center=(0, 0, 0), pos=(0, 0, 0)):
    """
    将东北天（ENU）坐标转换回WGS84坐标系的经纬度和高度。
    
    参数
    ----------
    center : tuple, optional
        建立ENU坐标系的原点的经纬度和高度, 形式为
        :math:`(lon, lat, h)`，by default (0, 0, 0)
    pos : tuple, optional
        需要转换的ENU坐标点, 形式为 :math:`(x, y, z)`
        by default (0, 0, 0)

    返回值
    ----------
    tuple
        返回的经纬度和高度，形式为 :math:`(lon, lat, h)`
    """
    
    # 解包元组中的值
    lon_center, lat_center, h_center = center
    x, y, z = pos
    
    # 地球半径（近似值）
    R = 6378137.0  # in meters
    
    # 将中心点的经纬度转换为弧度
    lat_center_rad = np.deg2rad(lat_center)
    lon_center_rad = np.deg2rad(lon_center)
    
    # 计算转换后的纬度和经度的差分
    delta_lat = y / R
    delta_lon = -x / (R * np.cos(lat_center_rad))
    
    # 将中心点的弧度加上差分，转换回角度
    lat = np.rad2deg(lat_center_rad + delta_lat)
    lon = np.rad2deg(lon_center_rad + delta_lon)
    
    # 计算高度
    h = z + h_center
    
    return lon, lat, h